#include <zephyr/kernel.h>
#include <zephyr/shell/shell.h>
#include <version.h>
#include <zephyr/logging/log.h>
#include <stdlib.h>
#include <string.h>
#include <zephyr/drivers/uart.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>
#include "include/sensor.h"
#include "include/common.h"

#include "include/wit_c_sdk.h"
#include "include/REG.h"
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>


LOG_MODULE_REGISTER(Sensor);

extern const struct device *uart_imu0;
extern const struct device *uart_imu1;
struct k_thread my_thread_data;
static volatile char cDataUpdate = 0, cCmd = 0xff; 
char stack_area[STACKSIZE];
static int gRcnt = 0;

struct device *pModbusUart = NULL;

// 如果需要，启用接收
uint8_t recv_buf[RECV_BUF_SIZE] = {0}; // 定义接收缓冲区
int iG_InterruptCnt = 0;
RingBuffer rb;
Sensor_Sta Sensor_Sta_g;
static void Delayms(uint16_t ucMs)
{
	k_sleep(K_MSEC(ucMs));
}

void ring_buffer_init(RingBuffer *rb) {
    rb->head = 0;
    rb->tail = 0;
    rb->size = RECV_BUF_SIZE;
}

bool ring_buffer_put(RingBuffer *rb, uint8_t data) {
    size_t next = (rb->head + 1) % rb->size;
    if (next == rb->tail) {  // 缓冲区已满
        return false;        // 添加数据失败
    }
    rb->buffer[rb->head] = data;
    rb->head = next;
    return true;
}

bool ring_buffer_get(RingBuffer *rb, uint8_t *data) {
    if (rb->head == rb->tail) {  // 缓冲区为空
        return false;            // 获取数据失败
    }
    *data = rb->buffer[rb->tail];
    rb->tail = (rb->tail + 1) % rb->size;
    return true;
}




static void SensorUartSend(uint8_t *p_data, uint32_t uiSize)
{
	for(int i=0;i<uiSize;++i)
	{
		uart_poll_out(pModbusUart,p_data[i]);
	}
}

static void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum)
{
	int i;
	//LOG_INF("Update!\n");
    for(i = 0; i < uiRegNum; i++)
    {
        switch(uiReg)
        {
//            case AX:
//            case AY:
            case AZ:
				cDataUpdate |= ACC_UPDATE;
            break;
//            case GX:
//            case GY:
            case GZ:
				cDataUpdate |= GYRO_UPDATE;
            break;
//            case HX:
//            case HY:
            case HZ:
				cDataUpdate |= MAG_UPDATE;
            break;
//            case Roll:
//            case Pitch:
            case Yaw:
				cDataUpdate |= ANGLE_UPDATE;
            break;
            default:
				cDataUpdate |= READ_UPDATE;
			break;
        }
		uiReg++;
    }
}

static void uart_cb(const struct device *dev, void *user_data)
{
	static uint8_t buf[1];
	static bool tx_busy;

	uart_irq_update(dev);

	if (uart_irq_rx_ready(dev)) {
		while (uart_fifo_read(dev, buf, sizeof(buf))) {
			if (!tx_busy) {
				uart_irq_tx_enable(dev);
			}
		}
		iG_InterruptCnt++;
		ring_buffer_put(&rb,buf[0]);
		//WitSerialDataIn(buf[0]);
	}

#if 1
	if (uart_irq_tx_ready(dev)) {
		if (!tx_busy) {
			(void)uart_fifo_fill(dev, buf, sizeof(buf));
			tx_busy = true;
		} else {
			tx_busy = false;
			uart_irq_tx_disable(dev);
		}
	}
#endif
}

void uart_irq_callback(const struct device *dev, void *user_data) {
    while (uart_irq_update(dev) && uart_irq_is_pending(dev)) {
        if (uart_irq_rx_ready(dev)) {
			gRcnt++;
            uint8_t buf[10];
            int recv_len = uart_fifo_read(dev, buf, sizeof(buf));
            if (recv_len > 0) {
                // 处理接收到的数据
				for(int i=0;i<recv_len;i++)
				{
					WitSerialDataIn(buf[i]);
				}
            }
        }
    }
}

void uart_rx_thread(void *a, void* un1, void* un2)
{ 
	unsigned char ucRecvChar = '2';
	
	ring_buffer_init(&rb);

	WitInit(WIT_PROTOCOL_NORMAL, 0x50);
	WitSerialWriteRegister(SensorUartSend);
	WitRegisterCallBack(CopeSensorData);
	WitDelayMsRegister(Delayms);

	uart_irq_callback_user_data_set(uart_imu0, uart_cb, NULL);
	uart_irq_rx_enable(uart_imu0);
	LOG_INF("enter rx task\n");
	//imu_rx_dispense(NULL, NULL, NULL);
} 

void imu_rx_dispense(void *p1, void *p2, void *p3){
	float fAcc[3], fGyro[3], fAngle[3];
	int i;
	int iAcc[3];
	int iGyro[3];
	int iAngle[3];
	uint8_t data;
	while(1){
			while (ring_buffer_get(&rb, &data)) {
				// 使用数据
				WitSerialDataIn(data);
			}
			
			if(cDataUpdate)
			{
				//LOG_INF("enter cDataUpdate\n");
				for(i = 0; i < 3; i++)
				{         
					fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
					fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
					fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
					
				}
				if(cDataUpdate & ACC_UPDATE)
				{
					Sensor_Sta_g.imu0.SAcc[0] = fAcc[0]*1000;
					Sensor_Sta_g.imu0.SAcc[1] = fAcc[1]*1000;
					Sensor_Sta_g.imu0.SAcc[2] = fAcc[2]*1000;
					// iAcc[0] = Sensor_Sta_g.imu0.SAcc[0];
					// iAcc[1] = Sensor_Sta_g.imu0.SAcc[1]; 
					// iAcc[2] = Sensor_Sta_g.imu0.SAcc[2]; 
					// LOG_INF("acc:%d %d %d\r\n", iAcc[0], 
					//                             iAcc[1], 
					// 							 iAcc[2]);
					cDataUpdate &= ~ACC_UPDATE;
				}
				if(cDataUpdate & GYRO_UPDATE)
				{
					Sensor_Sta_g.imu0.SGyro[0] = fGyro[0];
					Sensor_Sta_g.imu0.SGyro[1] = fGyro[1];
					Sensor_Sta_g.imu0.SGyro[2] = fGyro[2];
					// iGyro[0] = Sensor_Sta_g.imu0.SGyro[0];
					// iGyro[1] = Sensor_Sta_g.imu0.SGyro[1];
					// iGyro[2] = Sensor_Sta_g.imu0.SGyro[2];
					// LOG_INF("iGyro:%d %d %d\r\n", iGyro[0], 
					//                               iGyro[1],
					// 							  iGyro[2]);
					cDataUpdate &= ~GYRO_UPDATE;
				}
				if(cDataUpdate & ANGLE_UPDATE)
				{
					Sensor_Sta_g.imu0.SAngle[0] = fAngle[0]*100;
					Sensor_Sta_g.imu0.SAngle[1] = fAngle[1]*100;
					Sensor_Sta_g.imu0.SAngle[2] = fAngle[2]*100;
                    // iAngle[0] = Sensor_Sta_g.imu0.SAngle[0];
					// iAngle[1] = Sensor_Sta_g.imu0.SAngle[1];
					// iAngle[2] = Sensor_Sta_g.imu0.SAngle[2];
					// LOG_INF("iAngle:%d %d %d\r\n", iAngle[0], 
					//                                iAngle[1], 
					// 							   iAngle[2]);
					cDataUpdate &= ~ANGLE_UPDATE;
				}
				if(cDataUpdate & MAG_UPDATE)
				{
					//sprintf(tempbuf,"mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
					Sensor_Sta_g.imu0.SMag[0] = sReg[HX];
					Sensor_Sta_g.imu0.SMag[1] = sReg[HY];
					Sensor_Sta_g.imu0.SMag[2] = sReg[HZ]; 
					// LOG_INF("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
					cDataUpdate &= ~MAG_UPDATE;
				}
			}
			k_msleep(10);
		}
}

int imu_init(void){
    if (!device_is_ready(uart_imu0) ) {
        /* Not ready, do not use */
         if(!device_is_ready(uart_imu0)){
            LOG_ERR("imu uart device is not ready!");
        }
        return -ENODEV;
	}
	else{
    //   头部IMU陀螺仪串口设备
	    uart_rx_thread(NULL, NULL, NULL);
        return 0;
    }
}